#include "simple_nav_client.hpp"

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);

    auto nav_client = std::make_shared<simplenavclient>();

    // 示例：导航到坐标 (2.0, 1.0)，角度为 0 弧度
    if (nav_client->sendGoal(2.0, 1.0, 0.0)) {
        RCLCPP_INFO(nav_client->get_logger(), "等待导航完成...");

        if (nav_client->waitForResult(std::chrono::seconds(120))) {
            RCLCPP_INFO(nav_client->get_logger(), "导航任务成功完成！");
        } else {
            RCLCPP_ERROR(nav_client->get_logger(), "导航任务失败或超时");
        }
    } else {
        RCLCPP_ERROR(nav_client->get_logger(), "发送目标失败");
    }

    rclcpp::shutdown();
    return 0;
}